function UDP_Client(block)
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%
	setup(block);

function setup(block)

	air_period = 15;   %[ms]
	imu_period = 15;   %[ms]
	eng_period = 20;   %[ms]
	gps_period = 1000; %[ms]

	block.NumDialogPrms = 2;

	block.NumInputPorts = 15;
	block.NumOutputPorts = 0;

	%block.SetPreCompInpPortInfoToDynamic;
  %block.SetPreCompOutPortInfoToDynamic;

  block.InputPort(1).Complexity   = 'Real';
  block.InputPort(1).DataTypeId   = 1;
  block.InputPort(1).SamplingMode = 'Sample';
  block.InputPort(1).Dimensions   = 1;
  block.InputPort(1).SampleTime   = [air_period/1000 0];

  block.InputPort(2).Complexity   = 'Real';
  block.InputPort(2).DataTypeId   = 1;
  block.InputPort(2).SamplingMode = 'Sample';
  block.InputPort(2).Dimensions   = 1;
  block.InputPort(2).SampleTime   = [air_period/1000 0];

  block.InputPort(3).Complexity   = 'Real';
  block.InputPort(3).DataTypeId   = 1;
  block.InputPort(3).SamplingMode = 'Sample';
  block.InputPort(3).Dimensions   = 1;
  block.InputPort(3).SampleTime   = [air_period/1000 0];

  block.InputPort(4).Complexity   = 'Real';
  block.InputPort(4).DataTypeId   = 1;
  block.InputPort(4).SamplingMode = 'Sample';
  block.InputPort(4).Dimensions   = 1;
  block.InputPort(4).SampleTime   = [air_period/1000 0];

  block.InputPort(5).Complexity   = 'Real';
  block.InputPort(5).DataTypeId   = 4;
  block.InputPort(5).SamplingMode = 'Sample';
  block.InputPort(5).Dimensions   = 3;
  block.InputPort(5).SampleTime   = [imu_period/1000 0];

  block.InputPort(6).Complexity   = 'Real';
  block.InputPort(6).DataTypeId   = 4;
  block.InputPort(6).SamplingMode = 'Sample';
  block.InputPort(6).Dimensions   = 3;
  block.InputPort(6).SampleTime   = [imu_period/1000 0];

  block.InputPort(7).Complexity   = 'Real';
  block.InputPort(7).DataTypeId   = 3;
  block.InputPort(7).SamplingMode = 'Sample';
  block.InputPort(7).Dimensions   = 1;
  block.InputPort(7).SampleTime   = [imu_period/1000 0];

  block.InputPort(8).Complexity   = 'Real';
  block.InputPort(8).DataTypeId   = 5;
  block.InputPort(8).SamplingMode = 'Sample';
  block.InputPort(8).Dimensions   = 2;
  block.InputPort(8).SampleTime   = [eng_period/1000 0];

  block.InputPort(9).Complexity   = 'Real';
  block.InputPort(9).DataTypeId   = 7;
  block.InputPort(9).SamplingMode = 'Sample';
  block.InputPort(9).Dimensions   = 1;
  block.InputPort(9).SampleTime   = [eng_period/1000 0];

  block.InputPort(10).Complexity   = 'Real';
  block.InputPort(10).DataTypeId   = 3;
  block.InputPort(10).SamplingMode = 'Sample';
  block.InputPort(10).Dimensions   = 1;
  block.InputPort(10).SampleTime   = [gps_period/1000 0];

  block.InputPort(11).Complexity   = 'Real';
  block.InputPort(11).DataTypeId   = 3;
  block.InputPort(11).SamplingMode = 'Sample';
  block.InputPort(11).Dimensions   = 1;
  block.InputPort(11).SampleTime   = [gps_period/1000 0];

  block.InputPort(12).Complexity   = 'Real';
  block.InputPort(12).DataTypeId   = 5;
  block.InputPort(12).SamplingMode = 'Sample';
  block.InputPort(12).Dimensions   = 1;
  block.InputPort(12).SampleTime   = [gps_period/1000 0];

  block.InputPort(13).Complexity   = 'Real';
  block.InputPort(13).DataTypeId   = 3;
  block.InputPort(13).SamplingMode = 'Sample';
  block.InputPort(13).Dimensions   = 4;
  block.InputPort(13).SampleTime   = [gps_period/1000 0];

  block.InputPort(14).Complexity   = 'Real';
  block.InputPort(14).DataTypeId   = 6;
  block.InputPort(14).SamplingMode = 'Sample';
  block.InputPort(14).Dimensions   = 3;
  block.InputPort(14).SampleTime   = [gps_period/1000 0];

  block.InputPort(15).Complexity   = 'Real';
  block.InputPort(15).DataTypeId   = 4;
  block.InputPort(15).SamplingMode = 'Sample';
  block.InputPort(15).Dimensions   = 3;
  block.InputPort(15).SampleTime   = [gps_period/1000 0];

	%block.SampleTimes = [-1 0];

	block.RegBlockMethod('PostPropagationSetup',    @DoPostPropSetup);
	block.RegBlockMethod('Start',                   @Start);  
	block.RegBlockMethod('Outputs',                 @Outputs);
	block.RegBlockMethod('SetInputPortSampleTime',  @PortInputs);
	block.RegBlockMethod('SetOutputPortSampleTime', @PortOutputs);
	block.RegBlockMethod('Terminate',               @Terminate);

function DoPostPropSetup(block)
	block.NumDworks = 1;

	block.Dwork(1).Name            = 'socket';
	block.Dwork(1).Dimensions      = 1;
	block.Dwork(1).DatatypeID      = 0;      % double
	block.Dwork(1).Complexity      = 'Real'; % real
	block.Dwork(1).UsedAsDiscState = true;

function Start(block)
	port = block.DialogPrm(2).data;

	udp=pnet('udpsocket',port);
	pnet(udp,'setwritetimeout',5/1000);
	block.Dwork(1).Data = udp;

function PortInputs(block, port, time)
	block.InputPort(port).SampleTime = time;

function PortOutputs(block, port, time)
	block.OutputPort(port).SampleTime = time;

function Outputs(block)
	udp = block.Dwork(1).Data;

	air_type  = 2;
	imu_type  = 3;
	eng_type  = 4;
	gps_type  = 5;

	host = block.DialogPrm(1).Data;
	port = block.DialogPrm(2).Data;

	if block.InputPort(1).IsSampleHit || ...
		 block.InputPort(2).IsSampleHit || ...
		 block.InputPort(3).IsSampleHit || ...
		 block.InputPort(4).IsSampleHit
		
		dynamic_p = block.InputPort(1).Data;
		oat       = block.InputPort(2).Data;
		offset_p  = block.InputPort(3).Data;
		static_p  = block.InputPort(4).Data;

		airData = [uint8(air_type);
							 flipud(typecast(dynamic_p,  'uint8')');
							 flipud(typecast(oat,        'uint8')');
							 flipud(typecast(offset_p,   'uint8')');
							 flipud(typecast(static_p,   'uint8')')
		];

		if udp~=-1,
			try,
				pnet(udp,'write',airData);              % Write to write buffer
				pnet(udp,'writepacket',host,port,'noblock');   % Send buffer as UDP packet
			end
		else
			fprintf(1,'Socket not connected\n');
		end

% Due to some stupid thing in Simulink, it can't tell you when all of the ports have a sample hit if some of them have the same sample time.  Therefore since the air data rate is the same as the imu I have to fall through here.
%                      -- elstonj
%
%	end

%	if block.InputPort(5).IsSampleHit || ...
%		 block.InputPort(6).IsSampleHit || ...
%		 block.InputPort(7).IsSampleHit

		p         = block.InputPort(5).Data(1);
		q         = block.InputPort(5).Data(2);
		r         = block.InputPort(5).Data(3);
		ax        = block.InputPort(6).Data(1);
		ay        = block.InputPort(6).Data(2);
		az        = block.InputPort(6).Data(3);
		seq       = block.InputPort(7).Data;

		imuData = [uint8(imu_type);
							 flipud(typecast(p,          'uint8')');
							 flipud(typecast(q,          'uint8')');
							 flipud(typecast(r,          'uint8')');
							 flipud(typecast(ax,         'uint8')');
							 flipud(typecast(ay,         'uint8')');
							 flipud(typecast(az,         'uint8')');
							 seq
		];

		if udp~=-1,
			try,
				pnet(udp,'write',imuData);              % Write to write buffer
				pnet(udp,'writepacket',host,port);   % Send buffer as UDP packet
			end
		else
			fprintf(1,'Socket not connected\n');
		end

	end
			
	if block.InputPort(8).IsSampleHit || ...
		 block.InputPort(9).IsSampleHit
		rpm       = block.InputPort(8).Data;
		time      = block.InputPort(9).Data;

		engData = [uint8(eng_type);
							 flipud(typecast(rpm,        'uint8'));
							 flipud(typecast(time,       'uint8')')
		];

		if udp~=-1,
			try,
				pnet(udp,'write',engData);              % Write to write buffer
				pnet(udp,'writepacket',host,port);   % Send buffer as UDP packet
			end
		else
			fprintf(1,'Socket not connected\n');
		end

	end

	if block.InputPort(10).IsSampleHit || ...
		 block.InputPort(11).IsSampleHit || ...
		 block.InputPort(12).IsSampleHit || ...
		 block.InputPort(13).IsSampleHit || ...
		 block.InputPort(14).IsSampleHit || ...
		 block.InputPort(15).IsSampleHit

		gps_month = block.InputPort(10).Data;
		gps_day   = block.InputPort(11).Data;
		gps_year  = block.InputPort(12).Data;
		hms       = block.InputPort(13).Data;
		lat       = block.InputPort(14).Data(1);
		lon       = block.InputPort(14).Data(2);
		alt       = block.InputPort(14).Data(3);
		vx        = block.InputPort(15).Data(1);
		vy        = block.InputPort(15).Data(2);
		vz        = block.InputPort(15).Data(3);

		% FIXME -- need to convert to GPS time
		datevec = clock;
		gps_year = uint16(datevec(1));
		gps_month = uint8(datevec(2));
		gps_day = uint8(datevec(3));
		hms(1) = uint8(datevec(4));
		hms(2) = uint8(datevec(5));
		hms(3) = uint8(datevec(6));
		hms(4) = uint8((mod(datevec(6),1)*100.0));
		% FIXME -- need to convert to GPS time

		gpsData = [uint8(gps_type);
							 gps_month;
							 typecast(gps_day,           'uint8')';
							 flipud(typecast(gps_year,   'uint8')');
							 typecast(hms,               'uint8');
							 flipud(typecast(lat,        'uint8')');
							 flipud(typecast(lon,        'uint8')');
							 flipud(typecast(alt,        'uint8')');
							 flipud(typecast(vx,         'uint8')');
							 flipud(typecast(vy,         'uint8')');
							 flipud(typecast(vz,         'uint8')')
		];

		if udp~=-1,
			try,
				pnet(udp,'write',gpsData);              % Write to write buffer
				pnet(udp,'writepacket',host,port);   % Send buffer as UDP packet
			end
		else
			fprintf(1,'Socket not connected\n');
		end

	end

function Terminate(block)
	udp = block.Dwork(1).Data;
	pnet(udp,'close');
